package ch.ethz.fcl.input;

import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.File;
import java.io.FileReader;
import java.io.FileWriter;
import java.io.IOException;

import ch.ethz.fcl.Constant;
import ch.ethz.fcl.KinectContainer;
import ch.ethz.fcl.tools.Tools;

/**
 * depth map of all points on the table, notice that they are not the same
 * 
 * @author crazyfrog
 * 
 */
public class TableDepthMap {
	private KinectContainer container;

	private String groundDepthMapFile = "res/valuelab/grounddepthmap";
	private String histDepthMapFile = "res/valuelab/histdepthmap1";

	private short[][] groundDepthMap;
	private short[][] histDepthMap;

	public TableDepthMap(KinectContainer container) {
		this.container = container;
	}

	public void saveHistDepthMap() {
		saveTableDepthMap(histDepthMapFile);
	}

	public void saveTableDepthMap(String fileName) {
		try {
			File file = new File(fileName);
			if (!file.exists())
				file.createNewFile();

			FileWriter fw = new FileWriter(file.getAbsoluteFile());
			BufferedWriter bw = new BufferedWriter(fw);

			for (int row = 0; row < Coordinate.HEIGHT; row++) {
				for (int col = 0; col < Coordinate.WIDTH; col++) {
					float depth = container.getDepthImage().getDepth(col, row);
					bw.write((int) depth + " ");
				}
				bw.write("\n");
			}
			bw.flush();
			bw.close();
		} catch (IOException e) {
			e.printStackTrace();
		}
	}

	public short[][] readDepthMap(String fileName) {
		short[][] depthMap = new short[Coordinate.HEIGHT][Coordinate.WIDTH];

		try {
			File file = new File(fileName);

			if (!file.exists())
				saveTableDepthMap(fileName);

			FileReader fr = new FileReader(file.getAbsoluteFile());
			BufferedReader br = new BufferedReader(fr);

			String line = br.readLine();
			int row = 0;
			while (line != null) {
				String[] depths = line.split(" ");
				for (int col = 0; col < depths.length; col++) {
					depthMap[row][col] = Short.parseShort(depths[col]);
				}
				row++;
				line = br.readLine();
			}

			br.close();
			fr.close();
		} catch (IOException e) {
			e.printStackTrace();
		}

		return depthMap;
	}

	public short[][] getHeightMap() {
		short[][] groundDepthMap = getGroundDepthMap();
		short[][] currentDepthMap = getHistDepthMap();

		short[][] heightMap = new short[Coordinate.HEIGHT][Coordinate.WIDTH];

		// need to have 1 pixel off as opencv cannot work with exact the
		// boundary
		for (int row = 1; row < Coordinate.HEIGHT - 1; row++) {
			for (int col = 1; col < Coordinate.WIDTH - 1; col++) {
				// for we know the table depth should be around 1300 - 1350
				if (currentDepthMap[row][col] > Constant.MAX_TABLE_DEPTH
						|| currentDepthMap[row][col] == 0)
					heightMap[row][col] = 0;
				else {
					int boxDepth = currentDepthMap[row][col];
					int groundDepth = groundDepthMap[row][col];

					if ((groundDepthMap[row][col] > Constant.MAX_TABLE_DEPTH
							|| groundDepthMap[row][col] == 0 || groundDepthMap[row][col] < Constant.MIN_TABLE_DEPTH)
							&& currentDepthMap[row][col] < Constant.MAX_TABLE_DEPTH) {
						int inBoundRow = (int) Tools.inBound(row,
								Coordinate.TABLE_INBOUND_MIN_Y,
								Coordinate.TABLE_INBOUND_MAX_Y);
						int inBoundCol = (int) Tools.inBound(col,
								Coordinate.TABLE_INBOUND_MIN_X,
								Coordinate.TABLE_INBOUND_MAX_X);
						groundDepth = groundDepthMap[inBoundRow][inBoundCol];
					}

					short heightDiff = (short) (groundDepth - boxDepth);

					// if (Math.abs(heightDiff) <= Constant.MIN_HEIGHT
					// || Math.abs(heightDiff) >= Constant.MAX_HEIGHT)
					// heightMap[row][col] = 0;
					// else
					heightMap[row][col] = (short) (Math.abs(heightDiff));

				}
			}
		}
		return heightMap;
	}

	public short[][] getHistDepthMap() {
		if (!Constant.KINECT_AVAI) {
			if (histDepthMap == null)
				histDepthMap = readDepthMap(histDepthMapFile);
		} else {
			histDepthMap = new short[Coordinate.HEIGHT][Coordinate.WIDTH];

			for (int row = 0; row < Coordinate.HEIGHT; row++)
				for (int col = 0; col < Coordinate.WIDTH; col++)
					histDepthMap[row][col] = (short) container.getDepthImage()
							.getDepth(col, row);
		}
		return histDepthMap;
	}

	public short[][] getGroundDepthMap() {
		if (groundDepthMap == null)
			groundDepthMap = readDepthMap(groundDepthMapFile);
		return groundDepthMap;
	}

}
